/*
 * Math.cpp
 *
 *  Created on: Jun 27, 2009
 *      Author: dragon
 */

#include "../include/Matematica.h"

const double Matematica::MY_NAN = 999999;
const double Matematica::MY_PI = 3.14159265;
const double Matematica::MY_ZERO = 0.0000000001;
const double Matematica::limitRaio = 0.3;

Matematica::Matematica()
{
}

Matematica::~Matematica()
{
}

/*static*/
Coordenada3d
Matematica::polarToCartesiano(double dist, double angle1, double angle2)
{
	Coordenada3d cartesiano;

	cartesiano.Z(sin(angle2) * dist);
	double dmark = cos(angle2) * dist;
	cartesiano.Y(cos(-angle1) * dmark);
	cartesiano.X(sin(-angle1) * dmark);

	return cartesiano;
}

/*static*/
double
Matematica::distanciaRelativa(double distObj1, double distObj2,
							  double anguloObj1, double anguloObj2)
{
	anguloObj1 = Matematica::grau2Rad(anguloObj1);
	anguloObj2 = Matematica::grau2Rad(anguloObj2);

	VetorPolar v1(distObj1, anguloObj1);
	VetorPolar v2(distObj2, anguloObj2);

    VetorPolar vetor = Matematica::subtracaoVetor(v1, v2);

    return vetor.getComprimento();
    //    double angulo = anguloObj1 - anguloObj2;
    //    angulo *= (angulo < 0 ) ? -1 : 1;
    //    double dr = pow(distObj1, 2) + pow(distObj2, 2) - distObj1*distObj2*cos(angulo);
    //    return sqrt(dr);
}

/*static*/
double
Matematica::distancia(Coordenada2d obj1, Coordenada2d obj2)
{
    double dx, dy;

    dx = obj1.X() - obj2.X();
    dy = obj1.Y() - obj2.Y();

    return sqrt(dx*dx + dy*dy);
}

/*static*/
double
Matematica::alturaRelativa(double distObj, double angVertObj)
{
	return distObj*cos(angVertObj);
}

/*static*/
double
Matematica::anguloCos(double cosseno)
{
    double angulo = acos(cosseno);

    angulo =  angulo*180;
    angulo /= M_PI;

    return angulo;
}

/*static*/
double
Matematica::coeficienteAngular(Coordenada2d ponto1, Coordenada2d ponto2)
{
    return (ponto1.Y() - ponto2.Y())/(ponto1.X() - ponto2.X());
}

/*static*/
double
Matematica::normaVetor(Coordenada2d vetor)
{
	return sqrt( (vetor.X() * vetor.X()) + (vetor.Y() * vetor.Y()) );
}

/*static*/
double
Matematica::calcAngRotacao(Coordenada2d vetor1, Coordenada2d vetor2)
{
	double normaVetor1 = normaVetor(vetor1);
	double normaVetor2 = normaVetor(vetor2);
	double angulo = 0;
	if(normaVetor1 * normaVetor2 != 0)
	{
		angulo = (vetor1.X() * vetor2.X() + vetor1.Y() * vetor2.Y());
		angulo = fabs(acos(angulo)/normaVetor1/normaVetor2);

		/*Determinação do Sinal*/
		if( (vetor1.Y() * vetor2.X() - vetor1.X() * vetor2.Y()) < 0 )
		{
			angulo = - angulo;
		}
	}
	return angulo;
}

/*static*/
Coordenada2d
Matematica::coordenada2dObjetos(Coordenada2d ponto, double distanciaPontoObjeto,
                          double anguloPontoPlano, double anguloPontoObjeto)
{
    Coordenada2d posicao = Matematica::coordenada2dRelativa(distanciaPontoObjeto, anguloPontoObjeto);
    double dx = ponto.X() - posicao.X();
    double dy = ponto.Y() - posicao.Y();
    return Coordenada2d(dx, dy);
}

/*static*/
Coordenada2d
Matematica::coordenada2dRelativa(double distanciaPontoObjeto, double anguloPontoObjeto)
{
    double xCA = distanciaPontoObjeto * cos(anguloPontoObjeto);
    double yCO = distanciaPontoObjeto * sin(anguloPontoObjeto);
    Coordenada2d pos(xCA, yCO);
    return pos;
}

/*static*/
VetorPolar
Matematica::somaVetor(VetorPolar v1, VetorPolar v2)
{
    /* Transform to Cartesian coordinates
     */
    double x1 = sin(v1.getAngulo()) * v1.getComprimento();
    double y1 = cos(v1.getAngulo()) * v1.getComprimento();
    double x2 = sin(v2.getAngulo()) * v2.getComprimento();
    double y2 = cos(v2.getAngulo()) * v2.getComprimento();

    /* Sum
     */
    x1 += x2;
    y1 += y2;

    /* Transform back to polar coordinates
     */
    VetorPolar vetor(sqrt(x1*x1+y1*y1), atan2(x1,y1));

    /* Length should be always positive so
     * if negative then we change direction
     * instead
     */
    vetor = Matematica::ajusteComprimento(vetor);

    /* Adjust the angle to be between -180
     * and 180 degrees.
     */
    vetor.setAngulo(Matematica::limitandoAngulo(vetor.getAngulo()));

    return vetor;
}

/*static*/
VetorPolar
Matematica::subtracaoVetor(VetorPolar v1, VetorPolar v2)
{
    /* Transform to Cartesian coordinates
     */
    double x1 = sin(v1.getAngulo()) * v1.getComprimento();
    double y1 = cos(v1.getAngulo()) * v1.getComprimento();
    double x2 = sin(v2.getAngulo()) * v2.getComprimento();
    double y2 = cos(v2.getAngulo()) * v2.getComprimento();

    /* Subtract
     */
    x1 -= x2;
    y1 -= y2;

    /* Transform back to polar coordinates
     */
    VetorPolar vetor(sqrt(x1*x1+y1*y1), atan2(x1,y1));

    /* Length should be always positive so
     * if negative then we change direction
     * instead
     */
    vetor = Matematica::ajusteComprimento(vetor);

    /* Adjust the angle to be between -180
     * and 180 degrees.
     */
    vetor.setAngulo(Matematica::limitandoAngulo(vetor.getAngulo()));

    return vetor;
}

/*static*/
double
Matematica::grau2Rad(double angulo)
{
    return angulo*MY_PI/180;
}

/*static*/
double
Matematica::rad2Grau(double angulo)
{
    return angulo*180/MY_PI;
}


/*static*/
VetorPolar
Matematica::ajusteComprimento(VetorPolar vetor)
{
	if (vetor.getComprimento() < 0)
	{
		vetor.setComprimento(vetor.getComprimento()*(-1.0));
		vetor.setAngulo(vetor.getAngulo() + MY_PI);
	}
	return vetor;
}

/*static*/
double
Matematica::limitandoAngulo(double angulo)
{
	while ((angulo)>(MY_PI))
	{
		(angulo)-=(2*MY_PI);
	}
	while ((angulo)<(-MY_PI))
	{
		(angulo)+=(2*MY_PI);
	}
	return angulo;
}


/*static*/
double
Matematica::calcularRotacao(Coordenada2d v1, Coordenada2d v2)
{
	double nv1,nv2;
	double ang;
	nv1 = sqrt( pow(v1.X(),2) + pow(v1.Y(),2) );
	nv2 = sqrt( pow(v2.X(),2) + pow(v2.Y(),2) );

	if(nv1*nv2 == 0)
	{
		ang = -1000;
		return ang;
	}

	ang = acos( (v1.X() * v2.X() + v1.Y()*v2.Y()) /nv1/nv2);

	if ( v1.Y() * v2.X() - v1.X()*v2.Y() < 0 )
	{
		ang = -ang;
	}

	return ang;
}

/*static*/
int
Matematica::sinal(double valor)
{

	return (int)((valor + 0.00001) / fabs(valor + 0.00001));
}

/*static*/
int
Matematica::sinal(int valor)
{
	return (int)((valor + 0.00001) / fabs(valor + 0.00001));
}

double
Matematica::signum(double x)
{
	if(x == 0)
		return 0.0;
	else
	if(x > 0)
		return 1.0;
	else
	if(x < 0)
		return -1.0;

	return x;
}

/*static*/
double
Matematica::myfabs(double valor){
	if(Matematica::sinal(valor) < 0){
		return -valor;
	}
	return valor;
}

/*static*/
int
Matematica::myfabs(int valor){
	if(Matematica::sinal(valor) < 0){
		return -valor;
	}
	return valor;
}

int
Matematica::binary_search(std::vector<double> arr, double find)
{
	int middle = 0;
	int first,last;
	int end = arr.size()-1;
	first = 0;
	last = end;
	//std::cerr<<"find = "<<find<<" "<<arr[1]<<" "<<arr[2]<<" "<<arr[3]<<" "<<end<<std::endl;

	if(arr[first] == find){

		return first;
	}
	else
	if(arr[last] == find){

		return last;
	}
	else{
			while(first <  last)
			{
				middle = (first + last)/2; //aqui no caso estou fazendo um typecasting, irei truncar o valor do quociente para pegar somente a parte inteira.

				if( arr[middle] == find)
				{
					return middle;
				}
				if( arr[middle+1] > find){
					return -(middle+1);
				}
				else
				if(find > arr[middle])
				{
					first = middle+1;
				}
				else
				{
					last = middle;
				}
		    }
		}

		return -1;
}

bool
Matematica::isWithinArea(Coordenada2d point1,Coordenada2d point2)
{
	if( ((point1.X()-limitRaio) < point2.X()) && (point2.X() < point1.X()+limitRaio)
		&& (point2.Y() < point1.Y()+limitRaio) && ( point2.Y()  > point1.Y()-limitRaio)){
		return true;
	}
	else{
		return false;
	}

}

